#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
from __future__ import print_function

import nao_defines
import json

# 单例修饰器
def singleton(cls, *args, **kw):
    instances = {}
    def _singleton():
        if cls not in instances:
            instances[cls] = cls(*args, **kw)
        return instances[cls]
    return _singleton

# 因为数据库模型对象在提交后会失效等问题
# 用一个全局的单例的代理管理器来统一管理
# proxy 实例,用一个字典实现,
# 要求 proxy 的名称必须是全局唯一的,
# 结合 cam的name,nao的name都是全局唯一来使用
@singleton
class ProxyManger(object):
    def __init__(self):
        self._proxys={}

    def has(self,proxy_name):
        return proxy_name in self._proxys

    def add(self,name,proxy):
        if self.has(name):
            raise ValueError, "Already exits " + name
        self._proxys[name] = proxy

    def get(self,name):
        if not self.has(name):
            raise ValueError, "Not exits " + name
        return self._proxys[name]

    def delete(self,name):
        if not self.has(name):
            raise ValueError, "Not exits " + name
        if name.endswith("client"):
            client = self._proxys[name]
            nao_name, position, tmp = name.split('_')
            camProxyName = '_'.join([nao_name,position,"camProxy"])
            camProxy = self._proxys[camProxyName]
            camProxy.unsubscribe(client)
            del self._proxys[name]
        else:
            pass

    def __del__(self):
        for k in self._proxys.keys():
            self.delete(k)

proxyManger = ProxyManger()

import os
basedir = os.path.abspath(os.path.dirname(__file__))

from sqlalchemy import create_engine
sql_path = os.path.join(basedir, 'data.sqlite')
CREATE_ALL = False
if os.path.exists(sql_path):
    CREATE_ALL = False
else:
    CREATE_ALL = True
#engine = create_engine( 'sqlite:///' + sql_path, echo=True)
engine = create_engine( 'sqlite:///' + sql_path)

from sqlalchemy.orm import sessionmaker,scoped_session
Session = sessionmaker(bind=engine)
Session = scoped_session(Session) # ??
from sqlalchemy.ext.declarative import declarative_base

Base = declarative_base()

from sqlalchemy import Column,Integer,String,Float,DateTime,ForeignKey,Boolean
from sqlalchemy import orm
from sqlalchemy.orm import relationship
from naoqi import ALProxy
import motion
import almath

from mdh_nao import Nao as Kin
tmp = {}
class Nao(Base):
    __tablename__ = 'naos'
    id = Column(Integer,primary_key=True)
    name = Column(String(64),unique=True)
    color = Column(String(64),unique=False) # 颜色, red 或 blue
    IP = Column(String(64),default='127.0.0.1')
    PORT = Column(Integer,default=9559)
    connect = Column(Boolean,default=True)
    # 通过传递 uselist=False 变为一对一关系
    cams = relationship('Cam',backref='nao', lazy='immediate')
    arms = relationship('Arm',backref='nao', lazy='immediate')
    states = relationship('State',backref='nao', lazy='immediate')

    def __init__(self,name,ip,port):
        self.name = name
        self.IP = ip
        self.PORT = port
        self.cams.append( Cam(name=self.name, position=u'top'))
        self.cams.append( Cam(name=self.name, position=u'bottom'))

    def init_arms(self):
        self.arms.append(Arm(self.kin.Head))
        self.arms.append(Arm(self.kin.TopCamera))
        self.arms.append(Arm(self.kin.BottomCamera))
        self.arms.append(Arm(self.kin.LeftHand))
        self.arms.append(Arm(self.kin.RightHand))

    @orm.reconstructor
    def init_on_load(self):
        self.kin = Kin(str(self.name)) # 用于运动学求解
        # 加载相机工具坐标系
        tool = self.top_cam.tool
        if tool is not None:
            self.kin.TopCamera.tool = tool
        tool = self.bottom_cam.tool
        if tool is not None:
            self.kin.BottomCamera.tool = tool
        # 连接服务代理
        if self.connect:
            self.motionProxy = self._get_proxy("ALMotion")
            self.postureProxy = self._get_proxy("ALRobotPosture")
            self.touchProxy = self._get_proxy("ALTouch")
        # 设定默认参数
        self.frame = motion.FRAME_TORSO
        self.axisMask = almath.AXIS_MASK_ALL
        self.useSensorValues = True

    def _getPos(self,name):
        Pos = self.motionProxy.getPosition(name,
                                         self.frame,
                                         self.useSensorValues)
        Pos = np.array(Pos)
        Pos[:3] *= 1000 # api长度单位为m,本系统为 mm
        return Pos

    def _setPos(self,name,Pos,v=1.0):
        assert isinstance(Pos,np.ndarray)
        Pos[:3] *= 0.001 # api长度单位为m,本系统为 mm
        Pos = Pos.tolist()
        self.motionProxy.setPositions(name,
                                      self.frame,
                                      Pos,
                                      v,
                                      self.axisMask)
    def _getTf(self,name):
        if name == "TopCamera":
            return self.kin.TopCamera.fkine(self._getAngles("Head"))
        elif name == "BottomCamera":
            return self.kin.BottomCamera.fkine(self._getAngles("Head"))
        else:
            tf = self.motionProxy.getTransform(name,
                                              self.frame,
                                              self.useSensorValues)
            tf = np.array(tf).reshape((4,4))
            tf = np.matrix(tf)
            tf[0:3,3] *= 1000 # 默认长度单位为 mm
            return tf

    def _setTf(self,name,Tf,v):
        assert isinstance(Tf,np.matrix)
        Tf = np.array(Tf,dtype=np.float32)
        Tf[0:3,3] *= 0.001 # 默认长度单位为 mm, setTransforms 接受长度单位为 m
        Tf = Tf.flatten().tolist()
        self.motionProxy.setTransforms(name,
                                   self.frame,
                                   Tf,
                                   v,
                                   self.axisMask)
    # 关节空间
    def _setAngles(self,names,angles,fractionMaxSpeed=0.5):
        self.motionProxy.setAngles(names, angles, fractionMaxSpeed)

    def _getAngles(self,names):
        return self.motionProxy.getAngles(names, self.useSensorValues)

    def _check_Angles(self,names,angles):
        return nao_defines.check_Limits(names,angles)

    def check_LArm_Angles(self,angles):
        return nao_defines.check_LArm_Limits(angles)

    def check_RArm_Angles(self,angles):
        return nao_defines.check_RArm_Limits(angles)
    
    # 电机电流(电机刚性)
    def _setStiffnesses(self,jointNames,stiffnesses):
        self.motionProxy.setStiffnesses(jointNames,stiffnesses)

    def _getStiffnesses(self,jointNames):
        return self.motionProxy.getStiffnesses(jointNames)

    # 手部控制
    def _openHand(self,handName):
        self.motionProxy.openHand(handName)

    def _closeHand(self,handName):
        self.motionProxy.closeHand(handName)

    # 定制操作
    def rest(self):
        self.motionProxy.rest()

    def wakeUp(self):
        self.motionProxy.wakeUp()

    def isWakeUp(self):
        return self.motionProxy.robotIsWakeUp()

    def __repr__(self):
        return "<Nao(name='%s',ip='%s',port=%d>" % (self.name,self.IP,self.PORT)

    def __getattr__(self, item):
        if item == "top_cam":
            return self.cams[0]
        elif item == "bottom_cam":
            return self.cams[1]
        return super(Nao,self).__getattribute__(item)

    # Nao API
    def _get_proxy(self,proxy_name):
        if not self.connect:
            return None
        _proxy_name = self.name + proxy_name
        if not proxyManger.has(_proxy_name):
            ip = str(self.IP)
            port = self.PORT
            proxyManger.add(_proxy_name, ALProxy(proxy_name, ip, port))
        return proxyManger.get(_proxy_name)

    def findTr(self,w,h,unit,top_img=None,bottom_img=None,angles=None):
        top_cam = self.top_cam
        bottom_cam = self.bottom_cam
        if angles is None:
            motionProxy = self._get_proxy("ALMotion")
            angles = motionProxy.getAngles("Head",True)
        Tc_o = top_cam.findTr(top_img,w=w,h=h,unit=unit)
        if Tc_o is not None:
            Tb_c = self.kin.TopCamera.fkine(angles)
            return Tb_c*Tc_o,Tc_o,"top"
        Tc_o = bottom_cam.findTr(bottom_img,w=w,h=h,unit=unit)
        if Tc_o is not None:
            Tb_c = self.kin.BottomCamera.fkine(angles)
            return Tb_c*Tc_o,Tc_o,"bottom"
        return None,None,None

    @classmethod
    def get_by_name(cls,name):
        ss = Session()
        return ss.query(Nao).filter_by(name=name).first()

    @classmethod
    def get(cls,id):
        ss = Session()
        return ss.query(cls).get(id)

    def get_states(self,name=None,**kwargs):
        ss = Session()
        States = ss.query(State).filter(State.nao == self)
        if name is not None:
            States = States.filter(State.name == name)
        if "top" in kwargs and kwargs["top"]:
            States = States.filter(State._top_tr != u"None")
        if "bottom" in kwargs and kwargs["bottom"]:
            States = States.filter(State._bottom_tr != u"None")
        if "before" in kwargs and isinstance(kwargs["before"],datetime.datetime):
            States = State.filter(State.datetime < kwargs["before"])
        if "after" in kwargs and isinstance(kwargs["after"],datetime.datetime):
            States = State.filter(State.datetime > kwargs["after"])
        return States.all()

    def tr_top_bottom(self):
        tr_e_top = self.kin.TopCamera.tool
        tr_e_bottom = self.kin.BottomCamera.tool
        return np.linalg.inv(tr_e_top)*tr_e_bottom

    def angleInterpolation(self,names, angleLists, timeLists, isAbsolute=True):
        self.motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute)
        
import numpy as np
import time
from robot import r2t,t2r,transl

def json2tr(tr_json):
    if tr_json == u'None':
        return None
    else:
        tr = json.loads(tr_json)
        return np.mat(tr).reshape((4,4))

def tr2json(tr):
    if tr is None:
        return u'None'
    else:
        tr_json = json.dumps(tr.tolist())
        return tr_json

class Cam(Base):
    __tablename__ = "cams"
    id = Column(Integer,primary_key=True)
    name = Column(String(64),unique=True)
    position = Column(String(64))
    # 相机内参数
    fx = Column(Float,default=1.0)
    fy = Column(Float,default=1.0)
    cx = Column(Float,default=0.0)
    cy = Column(Float,default=0.0)
    k1 = Column(Float,default=0.0)
    k2 = Column(Float,default=0.0)
    p1 = Column(Float,default=0.0)
    p2 = Column(Float,default=0.0)
    k3 = Column(Float,default=0.0)
    # 标定单位
    # 相机中的1代表实际中的多少 mm,与标定时的标定板所用的单位有关
    unit = Column(Float,default=1.0)
    # 标定时使用的图像分辨率, 默认为 640x480
    width = Column(Integer,default=640)
    height = Column(Integer,default=480)
    # 相机的工具坐标系
    _tool = Column(String(4000),default="None")
    # 相关关联的对象
    nao_id = Column(Integer,ForeignKey('naos.id'))

    def __init__(self,name,position):
        assert position in [u'top',u'bottom']
        self.position = position
        self.name = '_'.join([name,position])

    def get_P(self,src):
        ratio = self.get_ratio(src)
        self.ratio = ratio
        K = self.mtx*self.ratio
        C = np.matrix(np.zeros((3,1)))
        P = np.hstack((K,C))
        return P

    @orm.reconstructor
    def init_on_load(self):
        self.tool = self.get_tool()
        self.mtx = self.get_cameraMatrix()
        self.dist = self.get_distCoeffs()
        self.ratio = 1.0
        # 运动学运算器
        self.kin = Kin("nao")
        self.robot = self.kin.Head
        if self.tool is not None:
            self.robot.tool = self.tool
        camProxyName = '_'.join([self.name, "camProxy"])
        if not proxyManger.has(camProxyName):
            camProxy = self.nao._get_proxy("ALVideoDevice")
            if camProxy is None: return None
            proxyManger.add(camProxyName,camProxy)

        clientName = '_'.join([self.name,"client"])
        if not proxyManger.has(clientName):
            camProxy = proxyManger.get(camProxyName)
            import vision_definitions as vd
            if self.position == u'top':
                index = vd.kTopCamera
            else:
                index = vd.kBottomCamera
            if self.width == 640 and self.height == 480:
                resolution = vd.kVGA    # 分辨率 VGA (640x480)
                FPS = 25
            elif self.width == 320 and self.height == 240:
                resolution = vd.kQVGA    # 分辨率 VGA (320x240)
                FPS = 25
            #resolution = vd.k4VGA    # 分辨率 VGA (1280x960)

            colorSpace = vd.kBGRColorSpace   # BGR 色域
            prams = (str(clientName+"_"),index,resolution,colorSpace,FPS)
            client = camProxy.subscribeCamera(*prams)
            proxyManger.add(clientName,client)
        self.camProxy = proxyManger.get(camProxyName)
        self.client = proxyManger.get(clientName)

    def fkine(self,q):
        return self.robot.fkine(q)

    def get_tool(self):
        return json2tr(self._tool)

    def set_tool(self,tool):
        self._tool = tr2json(tool)

    def get_cameraMatrix(self):
        mtx = np.eye(3)
        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = self.fx,self.fy,self.cx,self.cy
        return mtx

    def get_distCoeffs(self):
        dist = np.zeros((1,5))
        dist[0] = self.k1,self.k2,self.p1,self.p2,self.k3
        return dist

    def get_image(self):
        # 获取一幅实时图像
        naoImage = self.camProxy.getImageRemote(self.client)
        if naoImage is None :
            if hasattr(self,"_image"):
                return self._image # 如果连接失败就返回缓存的图像
            else:
                time.sleep(0.3)
                return self.get_image() #
        else:
            imageWidth = naoImage[0]
            imageHeight = naoImage[1]
            array = np.frombuffer(naoImage[6],dtype=np.uint8)
            image = array.reshape((imageHeight, imageWidth,3))
            self._image = image # 图像缓存
            return image

    def get_ratio(self,src):
        height, width = src.shape[:2]
        assert width/self.width == height/self.height # 保证是等比缩放
        ratio = width/float(self.width)
        return ratio

    def findTr(self,src=None,w=9,h=4,unit=15):
        '''
        从标定板获得从世界坐标系到相机坐标系的变换矩阵
        :param src:  标定图像
        :param w:  标定角点的宽点个数
        :param h:  标定角点的高点个数
        :param unit: 标定板的单位长,mm
        :return: Tr 从世界坐标系到相机坐标系的表换矩阵
        '''
        if src is None: src = self.get_image()
        if len(src.shape) == 3: height,width,depth = src.shape
        elif len(src.shape) == 2: height,width = src.shape
        else: raise ValueError
        ratio = self.get_ratio(src)
        objp = np.zeros((h*w,3), np.float32)
        objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
        #print("unit:",self.unit)
        objp = objp*unit/self.unit
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        gray = cv2.cvtColor(src,cv2.COLOR_BGR2GRAY)
        found, corners = cv2.findChessboardCorners(gray, (w,h),None)
        if not found: return None
        cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        cv2.drawChessboardCorners(src,(w,h),corners,found)
        mtx = self.get_cameraMatrix()*ratio # 相机参数矩阵要等比例缩放
        dist = self.get_distCoeffs() # 畸变矫正参数与图像缩放无关
        ret,rvec,tvec = cv2.solvePnP(objp, corners,mtx,dist)
        if not ret:
            return None
        else:
            tr = np.matrix(np.eye(4))
            tr[:3,:3] = cv2.Rodrigues(rvec)[0]
            tr[:3,3] = tvec * self.unit # 换算成 mm
            return tr

    def _get_p2d_from_p3d(self,p3d):
        '''
        获得相机坐标系下坐标为 p3d 的图像坐标 p2d
        :param p3d: 可以是齐次的也可以是非齐次的坐标
        :return: p2d [x,y].T
        '''
        imgPoint = cv2.projectPoints(p3d[:3,0].reshape((1,3,1)),np.zeros((3,1)),np.zeros((3,1)),self.mtx,self.dist)[0] # 投影到相机
        imgPoint = imgPoint[0,0]
        p2d = np.mat(imgPoint).T
        return p2d

    def get_p2d_from_p3d(self,p3d,Tb_c=None):
        '''
        获得机器人坐标系下坐标为 p3d 的图像坐标 p2d
        :param p3d: 可以是齐次的也可以是非齐次的坐标
        :return: p2d [x,y].T
        '''
        assert isinstance(p3d,np.matrix)
        if len(p3d) == 3:
            p3d = np.vstack((p3d,1))
        if Tb_c is None:
            motionProxy = self.nao._get_proxy("ALMotion")
            angles = motionProxy.getAngles("Head",True)
            Tb_c = self.fkine(angles)
        Tc_b = np.linalg.inv(Tb_c)
        pc = Tc_b*p3d
        #print("pc:",pc)
        p2d = self._get_p2d_from_p3d(pc)
        return p2d

class Arm(Base):
    __tablename__ = "arms"
    id = Column(Integer,primary_key=True)
    name = Column(String(64),unique=True)
    # 相机的工具坐标系
    _base = Column(String(4000),default="None")
    _tool = Column(String(4000),default="None")
    # 相关关联的对象
    links = relationship('Link',backref='arm', lazy='immediate')
    nao_id = Column(Integer,ForeignKey('naos.id'))

    def __init__(self, _robot):
        #assert isinstance(_robot,robot.Robot)
        self.robot = _robot
        self.name = _robot.name
        self.set_base(_robot.base)
        self.set_tool(_robot.tool)
        for index, link in enumerate(_robot.links):
            self.links.append(Link(link,index))

    @orm.reconstructor
    def init_on_load(self):
        self.base = json2tr(self._base)
        self.tool = json2tr(self._tool)
        self.robot = robot.Robot(base=self.base,tool=self.tool,name=str(self.name))
        links = Link.get_by_arm_id(self.id)
        links = [link.link for link in links]
        self.robot.links = links

    def set_base(self,base):
        self._base = tr2json(base)

    def set_tool(self,tool):
        self._tool = tr2json(tool)

    def set_Robot(self,Robot):
        pass

import robot

class Link(Base):
    __tablename__ = "links"
    id = Column(Integer,primary_key=True)
    index = Column(Integer,default=0)
    alpha = Column(Float,default=0.0)
    A = Column(Float,default=0.0)
    theta = Column(Float,default=0.0)
    D = Column(Float,default=0.0)
    sigma = Column(Float,default=0.0)
    DH = Column(Boolean,default=False)
    # 相关关联的对象
    arm_id = Column(Integer,ForeignKey('arms.id'))

    def __init__(self,link,index=0):
        assert isinstance(link,robot.Link)
        self.index = index
        self.alpha = link.alpha
        self.A = link.A
        self.theta = link.theta
        self.D = link.D
        self.sigma = link.sigma
        self.DH = link.convention == robot.Link.LINK_DH

    @orm.reconstructor
    def init_on_load(self):
        LINK_DH = robot.Link.LINK_DH if self.DH else robot.Link.LINK_MDH
        self.link = robot.Link(alpha=self.alpha,
                               A=self.A,
                               theta=self.theta,
                               D=self.D,
                               sigma=self.sigma,
                               convention=LINK_DH)

    @classmethod
    def get_by_arm_id(cls,arm_id):
        ss = Session()
        return ss.query(cls).filter_by(arm_id=arm_id).order_by(cls.index).all()

import datetime
state_dir = os.path.join(basedir, "data", "State")
import cv2

# Nao 机器人状态
# 包含各个关节的角度和保存的时间
# 保存的图片格式为 2016-12-19 13:26:02.793000 <nao_name> <top/bottom>.jpg
class State(Base):
    __tablename__ = 'states'
    id = Column(Integer,primary_key=True)
    name = Column(String(64),default="None")
    nao_id = Column(Integer,ForeignKey('naos.id'))
    # 记录时间
    datetime = Column(DateTime)

    # 图像标定板参数
    w = Column(Integer,default=12)
    h = Column(Integer,default=9)
    unit = Column(Float,default=20.0)

    # 标定板位姿
    _top_tr = Column(String(4000),default="None")
    _bottom_tr = Column(String(4000),default="None")
    # 关节角度 Body
    _angles = Column(String(4000),default="None")

    def __init__(self, name,nao,w=12,h=9,unit=20.0,now=None):
        assert isinstance(nao,Nao)
        if now is None:
            self.datetime = datetime.datetime.now()
        else:
            assert isinstance(now,datetime.datetime)
            self.datetime = now
        self.name = name
        self.w=w
        self.h=h
        self.unit=unit
        motionProxy = nao._get_proxy("ALMotion")
        angles = motionProxy.getAngles("Body", True)
        self.setAngles(angles)
        self.save_img(nao)

    def __repr__(self):
        return "<State(name='%s',nao='%s',datetime='%s'>" % (self.name,self.nao.name,str(self.datetime))

    @orm.reconstructor
    def init_on_load(self):
        self.angles = self.getAngles("Body")
        self.top_tr = self.getTr("top")
        self.bottom_tr = self.getTr("bottom")

    def save_img(self,nao,top_img=None,bottom_img=None):
        top_path = self._get_top_img_path(nao=nao)
        bottom_path = self._get_bottom_img_path(nao=nao)
        top_cam = nao.top_cam
        top_img = top_cam.get_image()
        cv2.imwrite(top_path,top_img)
        bottom_cam = nao.bottom_cam
        bottom_img = bottom_cam.get_image()
        cv2.imwrite(bottom_path,bottom_img)

    def setAngles(self,angles):
        angles_json = json.dumps(angles)
        self._angles = angles_json

    def getAngles(self,names="Body"):
        if hasattr(self,"angles"):
            angles = self.angles
        else:
            if self._angles == u"None":
                raise RuntimeError
            else:
                angles = json.loads(self._angles)
        if names == "Body":
            return angles
        elif names == "Head":
            return angles[0:2]
        elif names == "LArm":
            return angles[2:8]
        elif names == "LLeg":
            return angles[8:14]
        elif names == "RLeg":
            return angles[14:20]
        elif names == "RArm":
            return angles[20:26]

    def __get_img_path(self,postion,nao=None):
        if nao is None:
            nao = self.nao
        time_name = str(self.datetime).replace(':','.') # : 不能用作 win32 的文件名
        img_name = " ".join([time_name,self.name,nao.name,postion+'.jpg'])
        return os.path.join(state_dir,img_name)

    def _get_top_img_path(self,nao=None):
        return self.__get_img_path("top",nao=nao)

    def _get_bottom_img_path(self,nao=None):
        return self.__get_img_path("bottom",nao=nao)

    def get_img(self):
        nao = self.nao
        if nao is None: return None
        top_path = self._get_top_img_path()
        bottom_path = self._get_bottom_img_path()
        top_img = cv2.imread(os.path.join(state_dir,top_path))
        bottom_img = cv2.imread(os.path.join(state_dir,bottom_path))
        return top_img,bottom_img

    def getTr(self,position):
        if position == 'top':
            if self._top_tr == u"None":
                return None
            else:
                tr_json = self._top_tr
        else:
            if self._bottom_tr == u"None":
                return None
            else:
                tr_json = self._bottom_tr
        tr = json.loads(tr_json)
        return np.mat(tr).reshape((4,4))

    def setTr(self,position,tr):
        if tr is None:
            return None
        tr_json = json.dumps(tr.tolist())
        if position == 'top':
            self._top_tr = tr_json
        else:
            self._bottom_tr = tr_json

    def findTr(self,position,top_img=None,bottom_img=None):
        if top_img is None or bottom_img is None:
            images = self.get_img()
            if images is None: return None
            top_img,bottom_img = images
        if position == 'top':
            cam = self.nao.top_cam
            img = top_img
        else:
            cam = self.nao.bottom_cam
            img = bottom_img
        tr = cam.findTr(img,w=self.w,h=self.h,unit=self.unit)
        return tr

    def init_tr(self):
        self.top_tr = self.findTr('top')
        self.setTr('top',self.top_tr)
        self.bottom_tr = self.findTr('bottom')
        self.setTr('bottom',self.bottom_tr)

    @classmethod
    def remove_no_images(cls):
        ss = Session()
        for state in ss.query(cls).all():
            top_path = state._get_top_img_path()
            bottom_path = state._get_bottom_img_path()
            if os.path.exists(top_path) and os.path.exists(bottom_path):
                continue
            else:
                ss.delete(state)
        ss.commit()

    def get_obj(self):
        try:
            kin = self.nao.kin
        except:
            nao = Nao.get(self.nao_id)
            nao.kin.load_camera_tool()
            kin = nao.kin
        if self.top_tr is not None:
            return kin.TopCamera.fkine(self.getAngles("Head"))*self.top_tr
        elif self.bottom_tr is not None:
            return kin.BottomCamera.fkine(self.getAngles("Head"))*self.bottom_tr
        else:
            return None

def commit(objs):
    ss = Session()
    for obj in objs:
        ss.add(obj)
    ss.commit()

if CREATE_ALL:
    Base.metadata.create_all(engine)


